
#include <ros/ros.h>
#include "geometry_msgs/PoseStamped.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>
#include <deque>

ros::Publisher goal_pub;

nav_msgs::Odometry current_odom, last_odom;
Eigen::Vector3d current_dest;
std::deque<Eigen::Vector3d> cmd_positions;


int first_odom_flag = 0;
bool bArrived = 0;
void Odom_callback(const nav_msgs::Odometry& odom)
{
    if(first_odom_flag == 1)
    {
        last_odom = current_odom;
    }else{
        first_odom_flag = 1;
    }
    current_odom = odom;
    ROS_INFO("current odom received:%f,%f,%f",current_odom.pose.pose.position.x,current_odom.pose.pose.position.y,current_odom.pose.pose.position.z);
    Eigen::Vector3d currentPosition(current_odom.pose.pose.position.x,current_odom.pose.pose.position.y,current_odom.pose.pose.position.z); 

    // pub current dest
    geometry_msgs::PoseStamped goal_msg;
    goal_msg.header.frame_id="map";
    goal_msg.header.stamp = ros::Time::now();
    goal_msg.pose.position.x = current_dest(0);
    goal_msg.pose.position.y = current_dest(1);
    goal_msg.pose.position.z = current_dest(2);
    goal_msg.pose.orientation.w = 1;
    goal_msg.pose.orientation.x = 0;
    goal_msg.pose.orientation.y = 0;
    goal_msg.pose.orientation.z = 0;

    goal_pub.publish(goal_msg);
    ROS_INFO("current dest:%f,%f,%f",current_dest(0),current_dest(1),current_dest(2));

    if((currentPosition-current_dest).norm() < 5)
    {
        // change to next dest
        if(cmd_positions.size() > 0)
        {
            current_dest = cmd_positions.front();
            cmd_positions.pop_front();
        }
    }
}

int main(int argc, char** argv)
{
    ROS_WARN("*****START*****");
    ros::init(argc, argv, "positioncmd_generator_node");
    ros::NodeHandle n("~");

    // load trajectory
    std::string positioncmd_filepath;
    n.param("positioncmd_filepath", positioncmd_filepath, std::string(""));
    ROS_INFO("positioncmd_filepath:%s",positioncmd_filepath.c_str());

    // load cmd pcd 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cmd_points (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> (positioncmd_filepath.c_str(), *cmd_points) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
    }
    ROS_INFO("load cmds:%d",cmd_points->size());

    // get odometry
    ros::Subscriber odom_sub = n.subscribe("odom", 100, Odom_callback,
                                           ros::TransportHints().tcpNoDelay());

    goal_pub = n.advertise<geometry_msgs::PoseStamped>("cmd_pose", 10, true);

    for (size_t i = 0; i < cmd_points->size(); i++)
    {
        Eigen::Vector3d tmpPt;
        tmpPt(0) = cmd_points->points[i].x;
        tmpPt(1) = cmd_points->points[i].y;
        tmpPt(2) = cmd_points->points[i].z;
        cmd_positions.push_back(tmpPt);
    }
    current_dest = cmd_positions.front();
    cmd_positions.pop_front();
    ros::spin();
    return 0;
}